function new_particles = resample_mcl_particles(particles)
% 低方差重采样
% 输入: particles-粒子数组
% 输出: new_particles-重采样后粒子

    N = length(particles);
    weights = [particles.weight];
    
    new_particles = particles;
    
    r = rand() / N;
    c = weights(1);
    i = 1;
    
    for m = 1:N
        U = r + (m - 1) / N;
        while U > c
            i = i + 1;
            if i > N
                i = N;
                break;
            end
            c = c + weights(i);
        end
        
        new_particles(m) = particles(i);
        new_particles(m).weight = 1/N;
    end
end

